from imp import reload
import time

from test_init import *

import user_register

exec_file_path = current

# EMPTY TEST for no action
# only used to compile check & battery check

def sub_battery_info_handler(batter_info, ep_robot):
    percent = batter_info
    brightness = int(percent * 255 / 100)
    # print("Battery: {0}%.".format(percent))'

    if not config.show_battery_info_once:
        
        config.show_battery_info_once = True
        print("===========   Battery Print ================")
        print("")
        print("[localtime] {0}\n Battery = {1}%.".format(
            time.asctime(time.localtime()),
            percent))
        print("")
        print("===========Battery Print Success!===========")

    if (percent < 20): 
        ep_robot.led.set_led(comp="all", r=100, g=0, b=0)
        print("Battery: {0}%. 请充电！！请充电！！请充电！！".format(percent))
    else:
        pass
        # ep_robot.led.set_led(comp="all", r=brightness, g=brightness, b=brightness)


sensor_log = [0]*4
def sub_sensor_info_handler(sub_info):
    global sensor_log
    sensor_log = sub_info[:]
    # d = sub_info
    # print("[+ Sim] tof1:{0}  tof2:{1}  tof3:{2}  tof4:{3}".format(d[0], d[1], d[2], d[3]))
    pass

"""
useless: sub_position always reutrn 0 for the Z Component
[link] https://github.com/dji-sdk/RoboMaster-SDK/issues/39

in main()
ep_chassis.sub_position(freq=10, callback=sub_position_handler)
print_position_info()
ep_chassis.unsub_position()
"""
pos = [0,0,0]
def sub_position_handler(position_info):
    global pos
    pos = position_info
    print_position_info()

def print_position_info():
    print("chassis position: x:{0:.3f}, y:{1:.3f}, z:{2:.3f}".format(pos[0], pos[1], pos[2]))

def exec_usr_func(ep_robot):
    # file_name = input("filename = ")
    # file_name = "user_register.py"

    # file_name = exec_file_path + '/' + file_name
    # with open(file_name, 'r') as file:
    #     exec(compile(file.read(), file_name, 'exec'))
    # return
    
    # pf.physical_platform_initialize(ep_robot)
    input("wait")
    reload(user_register)
    user_register.run(ep_robot)


if __name__ == '__main__':
    sdk_version = version.__version__
    print("sdk version:", sdk_version)

    # robomaster.enable_logging_to_file()
    # robomaster.logger.setLevel(robomaster.logging.ERROR)
    
    ep_robot = pf.get_initialized_robot()

    # ep_robot = robot.Robot()
    # ep_robot.initialize(conn_type="sta")
    # ep_robot.set_robot_mode(mode=robot.CHASSIS_LEAD)
    
    print("===========connect success!===========")

    time.sleep(0.5)
    ep_version = ep_robot.get_version()
    print("- Robot Version: {0}".format(ep_version))
    time.sleep(0.5)

    # 低电量提示 - 电量<20% 在控制台提示充电
    ep_battery = ep_robot.battery
    ep_battery.sub_battery_info(5, sub_battery_info_handler, ep_robot)
    # time.sleep(0.5)

    # # 测试修改后的 sensor API
    # print("=========== Sensor Test ===========")
    ep_sensor = ep_robot.sensor
    ep_sensor.sub_distance(freq=50, callback=sub_sensor_info_handler)
    
    ep_chassis = ep_robot.chassis
    # input("pause>")
    # ep_chassis._dij_move(0,0,360,0,60).wait_for_completed()
    # while True:
    #     msg = input(">")
    #     if (msg=='q'): break
    #     if (msg=='exec'):
    #         exec_usr_func(ep_robot)
    # time.sleep(30)
    
    print(">>>start")
    exec_usr_func(ep_robot)

    # ep_chassis.move(0.2,0,0,0.5,0).wait_for_completed()

    # ep_chassis._dij_move(0.5,0,0,0.1,0).wait_for_completed()


    # ep_chassis._dij_move(-0.5,0,0,0.1,0).wait_for_completed()
    

    # ep_chassis.sub_position(freq=50, callback=sub_position_handler)
    # time.sleep(1)
    # ep_chassis.unsub_position()
    
    print("=========== Chassis: move() end ===========")
    time.sleep(1.5)
    
    ep_sensor.unsub_distance()
    # print("=========== Chassis Test ===========")
    # ep_chassis.move(0.3,0,0,0.3,0).wait_for_completed()
    # ep_chassis.move(-0.3,0,0,0.3,0).wait_for_completed()

    config.show_battery_info_once = False
    time.sleep(1)
    ep_battery.unsub_battery_info()
    ep_robot.close()
    
